// Fill out your copyright notice in the Description page of Project Settings.

#pragma once

#include "WorldMath.h"
#include "WorldArcIterator.h"
#include "WorldSpiralIterator.h"
#include "WorldEntityAxis.h"
#include "entities/WorldRoad.h"

/**
 * Iterate through the road reference line and emit cubic hermite spline spans.
 */
class FWorldHermiteIterator
{
public:

	// Reference: https://en.wikipedia.org/wiki/Cubic_Hermite_spline
	struct Interval
	{
		glm::dvec3	Param;
		glm::dvec3	P0;
		glm::dvec3	M0;
		glm::dvec3	P1;
		glm::dvec3	M1;
		glm::dbox3	BBox;
	};

public:

	FWorldHermiteIterator(const std::vector<FWorldRoadGeometry::Ptr>& geoms,
							const FWorldEntityAxis& regionAxis,
							const FWorldEntityAxis& roadAxis,
							const glm::dmat4x4& parentMat = glm::dmat4x4(1.0));

	bool HasNext() const;
	void Next();

	Interval GetInterval();

private:

	void NextGeometry();
	void BeginGeometry();
	void Polyfit();

private:

	const std::vector<FWorldRoadGeometry::Ptr>& _geoms;
	const FWorldEntityAxis& _regionAxis;
	const FWorldEntityAxis& _roadAxis;
	const glm::dmat4x4 _parentMat;

	FWorldRoadGeometryBase::Ptr	_geom;
	size_t _geomIdx;
	FWorldArcIterator _arcIt;
	FWorldSpiralIterator _spiralIt;
	int _intervalIdx;

	glm::dvec3 _pointRaw;
	glm::dvec3 _pointOnXY;
	double _hdg;
	double _s;
	double _length;
	glm::dmat3x3 _uv2xy;
	glm::dvec3 _pPrev;
	glm::dvec3 _tPrev;

	std::vector<double> _fitU;
	std::vector<double> _fitV;
	std::vector<double> _fitT;
	std::vector<double> _fitS;
	int _fitN;
	int _fitBegin;
	int _fitEnd;
	glm::dvec4 _fittedU;
	glm::dvec4 _fittedV;
	glm::dvec2 _fittedS;

};
